#include "ros/ros.h"
#include "cyberarm_controller/arm_state_msg.h"
#include "move_interface.hpp"
#include "get_state.hpp"
#include <ncurses.h>
#include "iostream_operation.hpp"

//! 暂时放在这里，目前仅有这一个节点用到了按键
#define KEYCODE_D 0x64
#define KEYCODE_Q 0x71
#define KEYCODE_RIGHT 0x43
#define KEYCODE_LEFT 0x44
#define KEYCODE_UP 0x41
#define KEYCODE_DOWN 0x42
#define KEYCODE_B 0x62
#define KEYCODE_C 0x63
#define KEYCODE_E 0x65
#define KEYCODE_F 0x66
#define KEYCODE_G 0x67
#define KEYCODE_R 0x72
#define KEYCODE_T 0x74
#define KEYCODE_V 0x76

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    initscr(); // 初始化ncurses
    timeout(0); // 设置为非阻塞输入
    ros::init(argc, argv, "test_subscribe_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("arm_state", 1000, GetState);
    char c;
    std::vector<std::vector<double>> pos_map;

    ros::Rate r(10);

    while(ros::ok())
    {
        c = getch();
        if(c == KEYCODE_D)
        {
            ROS_INFO_STREAM("record pos = " << current_state.pos_estimate[0] << ", " << current_state.pos_estimate[1] << ", " << current_state.pos_estimate[2] << ", " << current_state.pos_estimate[3]);
            pos_map.push_back(std::vector<double>(current_state.pos_estimate.begin(), current_state.pos_estimate.end()));
        }
        else if(c == KEYCODE_Q)
        {
            ROS_INFO("exit...");
            break;
        }
        
        r.sleep();
        ros::spinOnce();
    }

    std::string ss;
    for(unsigned int i = 0; i < pos_map.size(); i++)
    {
        for(unsigned int j = 0; j < MOTOR_NUM; j++)
        {
            ss += std::to_string(pos_map[i][j]) + ",";
        }
        ss += "\n";
    }

    ROS_INFO_STREAM(ss);
    ros::Rate wait(0.5);
    wait.sleep();

    double time = 2;
    double sample_rate = 200.0;
    ros::Rate wait_0_5s(2);
    for(unsigned int i = 0; i < pos_map.size(); i++)
    {
        MoveJ(pos_map[i], time, sample_rate);
        wait_0_5s.sleep();
    }
    endwin(); // 结束ncurses
    WriteToFile2D(pos_map, "teach_move_point.offt", ",");
    
    return 0;
}

